#include "gsensor.h"
#include <string.h>
#include <stdlib.h>
#include <stdbool.h>
#define LOG_TAG TAG_SNSR
#include "elog.h"
#include "gs_algorithm.h"
#include "../service/include/swei_alarm.h"
#include <time.h>
#include "FreeRTOS.h"
#include "task.h"
#include "projdefs.h"
#include "cmsis_os2.h"
#include "bsp.h"
#include "qlcommon.h"
#include <math.h>
#include "drivers/sensor/sensor_support.h"
#include <sensor/skeleton/sensor_support_chip.h>

#include "comm/co_heartbeat.h"
#include <sensor/icm42670/sensor_icm42670.h>
#include "service/include/swei_config.h"
#include "misc/misc_system.h"
#include "service/include/swei_location.h"
#include "gnss_power_mgr.h"


typedef struct
{   
    uint8_t power_state;        /* 0 - 关机, 1 - 开机 */
    uint8_t auth_state;         /* 0: auth fail/not yet; 1: auth ok */
    uint32_t alarm_state;       /* 允许休眠时间 */
    uint32_t move_alarm_count;  /* 震动次数 */
    uint32_t unusal_move_alarm_count;   /* 异动次数 */
    uint32_t tilt_alarm_count;         
    uint32_t roll_move_alarm_count;    
}dev_sta_t;

dev_sta_t dev = {0};    /* 记录dev状态 */
utils_time_t move_detect_timer = {0};   /* */
utils_time_t unusual_detect_timer = {0};

/*卡尔曼滤波的参数*/
static ST_KALMAN_FILTER_PARA KF_pitch;
static ST_KALMAN_FILTER_PARA KF_roll;

/*加速度计的初始读入数据*/
static struct FW_GSENSOR_INFO_STRU g_info = { 0 };

/*通过低通滤波器后进入卡尔曼算法的加速度*/
static struct FW_GSENSOR_INFO_STRU kg_info = { 0 };

/*陀螺仪的初始读入数据*/
static struct FW_GYROSCOPE_INFO_STRU gy_info = { 0 };

/*陀螺仪通过高通滤波后的数据*/
static struct FW_GYROSCOPE_INFO_STRU kgy_info = { 0 };

/*平台配置参数*/
static ST_GSENSOR_DRIVER_PARA gsensor_driver_para = {2, 1, 8, 100, 3, 50};

/*倾斜检测相关阈值*/
static ST_GSENSOR_MNG_THRSHOLID  tilt_mng_thresh = {0};

/*翻转检测相关阈值*/
static ST_GSENSOR_MNG_THRSHOLID  overturn_mng_thresh = {0};

/*角度*/
static ST_ANGLE st_angle = {0};

/*加速度*/
static ST_ACC st_acc;

/*加速度变化率*/
static ST_RATE_ACC st_rate_acc;

/*异动gps数据*/
static ST_GPS_MOVE_CHECK_INFO gps_check_info;


/*当前状态*/
#warning " Gsensor state not processed with power on/off event. set default under power on status."
static volatile int gs_collide_machine = GSENSOR_LOW_POWER; 


/*震动/异动告警触发间隔 5min*/
int gs_tremos_thresh_during_time = 300;
int gs_unusual_moving_thresh_during_time = 300;

/*速度队列，速度判断用前第80次，之后可以进行调参数,7s*/
static int speed_tick_queue[SPEED_TICK_COUNT] = { 0 };
static int speed_tick_last = 0;
static int tick_speed = 0;

static int gs_collide_time_during = 5;/*碰撞事件接收时间间隔*/

static int gs_collide_event_count = 0;/*碰撞事件发生的次数*/

/*记录最近3个异动事件间隔*/
static int gs_collide_event_time_queue[3] = {0};

/*上次进入震动报警处理的时间*/
static int gs_tremos_last_time = 0;

/*刹车亮尾灯判断时间间隔*/
// static int g_brake_prev_time = 0;

/*震动报警声音播放flag*/
static bool is_collide_sound_playing = false;

static int gs_unusual_move_warn_last_time = 0; /*上次进行异动gps检测的时间*/
static bool gs_gps_check_move_thread_flag = false; /*异动检测的，gps线程是否进行的标志位*/
static int test_over_flag = 0; /*手动测试gps异动标志位*/
static int gs_unusual_move_log_time_prev = 0; /*异动检测控制刷屏log标志位*/

static int tilt_count = 0; /*倾斜检测的总次数*/
static int tilt_tick_first = 0; /*连续检测时，第一次倾斜的时间*/
static int tilt_state_now = TILT_SAFE; /*当前检测时的状态*/
static int tilt_state_last = TILT_SAFE; /*上次检测时的状态*/
static int s_tilt_warnning_last_time = 0; /*上次倾斜报警的时间*/
int gs_tilt_warnning_during_time = 300;  /* 目前要求5分钟只报一次 */




/*翻滚相关参数*/
static int overturn_count = 0;
static int overturn_tick_first = 0;
static int overturn_state_now = TILT_SAFE;
static int overturn_state_last = TILT_SAFE;
static int s_overturn_warning_last_time = 0;
int gs_overturn_warnning_during_time = 300;  /* 目前要求5分钟只报一次 */


/*计数器*/
static int tick = 0;

/*控制打印频率flag*/
static bool debug_status = FALSE; 

/*控制CAN数据广播接收标志位*/
bool g_flag_ltne = false;

/*当前档位*/
//static int current_gears = 0;

/*此标志位作用为防止异动检测过程中遇到RTC等导致睡眠情况的发生*/
static bool unusual_move_set_bussiness_flag = FALSE;

/*为使异动告警更易触发设置阈值*/
const int unusual_move_thresh = SENSITIVITY_VERY_HIGH_LEVEL;

/*是否由于异动告警设置极低阈值标志位*/
static bool set_gsensor_thresh_flag = FALSE;



/* Gsensor sensitivity port */
/* Gsensr数据缓存 */
static sensor_middle_support_t sensor_middle_support = {0};


ST_GSENSOR_DRIVER_PARA *get_gsensor_local_run_config(void)
{
    return &gsensor_driver_para;
}


ST_GSENSOR_MNG_THRSHOLID *get_gsensor_tilt_run_config(void)
{
    return &tilt_mng_thresh;
}


ST_GSENSOR_MNG_THRSHOLID *get_gsensor_overturn_run_config(void)
{
    return &overturn_mng_thresh;
}



int sensor_source_data_is_update(void)
{
    return (true == sensor_middle_support.update) ? 1 : 0;
}


accel_src_t* accel_src_buffer_get(void)
{
    return (accel_src_t*) sensor_middle_support.accel;
}

unsigned int accel_src_buffer_wr_index_get(void)
{
    return sensor_middle_support.accel_index;
}

void accel_src_buffer_wr_index_set(unsigned int idx)
{
    if (idx < SENSOR_ACCEL_SRC_BUFFER_CONTAIN)
        sensor_middle_support.accel_index = idx;
}


unsigned int accel_src_buffer_max_count(void)
{
    return SENSOR_ACCEL_SRC_BUFFER_CONTAIN;
}

/*******************************************************************************
* @brief       设置震动触发等级(1-3级 )
* @param[in]   glevel 震动触发等级(1-3级 )
* @param[out]  none
* @return      none
 *******************************************************************************/
void sensor_motion_on_accel_threshold_set(int glevel)
{ 
    if(glevel == SENSITIVITY_VERY_HIGH_LEVEL) {
        gsensor_driver_para.tremos_sensitivity = glevel;  
    } else {
        if (glevel < SENSITIVITY_LOW_LEVEL)
            glevel = SENSITIVITY_LOW_LEVEL;

        if(glevel > SENSITIVITY_HIGH_LEVEL)
            glevel = SENSITIVITY_HIGH_LEVEL;
        
        gsensor_driver_para.tremos_sensitivity = glevel;  
    }
}



/*******************************************************************************
* @brief       获取震动触发阈值(单位G, 0-2级 每级为0.2G)
* @param[in]   none
* @param[out]  none
* @return      none
 *******************************************************************************/
float sensor_motion_on_accel_threshold_get(void)
{
    if (gsensor_driver_para.tremos_sensitivity == SENSITIVITY_VERY_HIGH_LEVEL) {
        return SENSITIVITY_VERY_HIGH_VALUE / 1000.0f;
    } else { 
        return (SENSITIVITY_HIGH_LEVEL - gsensor_driver_para.tremos_sensitivity + 1) * SENSITIVITY_PER_CLASS_VALUE / 1000.0f;  
    }
}


/*******************************************************************************
* @brief       设置低功耗唤醒触发等级(0-255级 )
* @param[in]   glevel 唤醒灵敏度(0-255级 )
* @param[out]  none
* @return      none
 *******************************************************************************/
void sensor_wakeup_motion_accel_threshold_set(int int_sensitivity)
{ 
    if( (int_sensitivity >= 0) && (int_sensitivity <= 255) ) {
        gsensor_driver_para.interrupt_sensitivity = int_sensitivity;
    }
 
}



/*******************************************************************************
* @brief       获取低功耗唤醒触发阈值(单位G, 0-255级划分满量程(2 4 8 16G))
* @param[in]   none
* @param[out]  none
* @return      none
 *******************************************************************************/
float sensor_wakeup_motion_accel_threshold_get(void)
{
    float sensor_adc_per_mg = get_gsensor_local_run_config()->measure_range*1000.0f/icm42670_sensor_max_accel_value();
    
    if (gsensor_driver_para.interrupt_sensitivity == 0) {
        return SENSITIVITY_VERY_HIGH_VALUE / 1000.0f;  //0.02g
    } else { 

        return (255 - gsensor_driver_para.interrupt_sensitivity + 1) * get_gsensor_local_run_config()->measure_range * 1.0f / 255;
    }
}



/*******************************************************************************
* @brief       获取震动状态判断的加速度阈值和触发次数阈值
* @param[in]   none
* @param[out]  none
* @return      none
 *******************************************************************************/
static unsigned int event_motion_threshold_get(unsigned int *motion_count_thr)
{
    float motion_config_thr  = 0.0;
    unsigned int motion_thr  = 0;

    motion_config_thr = sensor_motion_on_accel_threshold_get();
   
    if (motion_config_thr < 0.02)
        motion_config_thr = 0.02;

    /* 计算震动阈值 */
    motion_thr = (unsigned int) (motion_config_thr * sensor_accel_1g_range_count());

    *motion_count_thr = EVENT_MOTION_COMPARE_STABLE_CNT;
  

    return motion_thr;
}


/*******************************************************************************
* @brief       动静状态判断
* @param[in]   none
* @param[out]  none
* @return      none
 *******************************************************************************/
unsigned int motion_detech_variation(void)
{
    unsigned int i = 0;
    unsigned int j = 0;
    unsigned int prev_idx = 0;
    unsigned int next_idx = 0;
    unsigned int motion_count = 0;
    unsigned int motion_count_thr = 0;
    unsigned int motion_threshold = event_motion_threshold_get(&motion_count_thr);
    unsigned int accel_max_count  = accel_src_buffer_max_count();
    unsigned int accel_write_idx  = accel_src_buffer_wr_index_get();
    const accel_src_t *accel_buff = accel_src_buffer_get();
    accel_src_t accel_data = { 0 };

    /* 没有分配到空间 */
    if (NULL == accel_buff)
        return MOTION_STATUS_NONE;


    if (accel_write_idx >= (EVENT_MOTION_COMPARE_COUNT + 1)) {
        accel_write_idx = accel_write_idx - EVENT_MOTION_COMPARE_COUNT - 1;
    } else {
        accel_write_idx = accel_write_idx + accel_max_count - EVENT_MOTION_COMPARE_COUNT - 1;
    }

    prev_idx = accel_write_idx;
    for (i = 0; i < EVENT_MOTION_COMPARE_COUNT - 1; i++) {
        prev_idx = prev_idx + 1;
        if (prev_idx >= accel_max_count)
            prev_idx = 0;
        if ((0 == accel_buff[prev_idx].x) && (0 == accel_buff[prev_idx].y) && (0 == accel_buff[prev_idx].z)) {
            continue;
        }
        next_idx = prev_idx;
        for (j = i + 1; j < EVENT_MOTION_COMPARE_COUNT; j++) {
            next_idx = next_idx + 1;
            if (next_idx >= accel_max_count)
                next_idx = 0;
            if ((0 == accel_buff[next_idx].x) && (0 == accel_buff[next_idx].y) && (0 == accel_buff[next_idx].z)) {
                continue;
            }

            /* 外力促使水平和重力方向发生加速度大于触发水平时(根据灵敏度, 0.2G一个等级) */
            accel_data.x = abs(accel_buff[prev_idx].x - 0);
            accel_data.y = abs(accel_buff[prev_idx].y - 0);
            accel_data.z = abs(accel_buff[prev_idx].z - sensor_accel_1g_range_count());
            
            if ((accel_data.z > motion_threshold) || (accel_data.y > motion_threshold) || (accel_data.x > motion_threshold)) {
                motion_count++;
            }
        }
    }

    if (motion_count >= motion_count_thr) {
        return MOTION_STATUS_ON;
    } else {
        return MOTION_STATUS_OFF;
    }
}


/****************************************************初始化**********************************************************/
void gsensor_thresh_init(void)
{
    tilt_mng_thresh.acc= 500.0f;  //0.5g
    tilt_mng_thresh.angle= 40;
    tilt_mng_thresh.gyro = 90;  //90度/S
    tilt_mng_thresh.time= 2;
    tilt_mng_thresh.vel = 10;  //10km/h

    overturn_mng_thresh.acc = 1000.0f;  //1g
    overturn_mng_thresh.angle = 20;
    overturn_mng_thresh.gyro = 180;  //180度/S
    overturn_mng_thresh.time = 1;
    overturn_mng_thresh.vel = 10;  //10km/h

    gsensor_driver_para.interrupt_sensitivity = 6;   //8G/255=0.0314g  6*0.0314g high
    gsensor_driver_para.tremos_sensitivity = 1;      //standard 0.4G
    gsensor_driver_para.measure_range = 8;  //8G
    gsensor_driver_para.sample_frequecy = 100;  //HZ
    gsensor_driver_para.reading_count = 3;
    gsensor_driver_para.interval = 50;  //ms

    gs_tremos_thresh_during_time = 300;
    gs_unusual_moving_thresh_during_time = 300;
    gs_tilt_warnning_during_time = 300; 
    gs_overturn_warnning_during_time = 300;

}

void gsensor_thresh_print(void)
{

    log_i("tilt_mng_thresh acc=%f, angle=%d, gyro=%d, time=%d, vel=%d",
        tilt_mng_thresh.acc, tilt_mng_thresh.angle, tilt_mng_thresh.gyro, tilt_mng_thresh.time, tilt_mng_thresh.vel);
    
    log_i("overturn_mng_thresh acc=%f, angle=%d, gyro=%d, time=%d, vel=%d",
        overturn_mng_thresh.acc, overturn_mng_thresh.angle, overturn_mng_thresh.gyro,
        overturn_mng_thresh.time, overturn_mng_thresh.vel);

    log_i("shaking thresh duringtime=%d, unusual moving thresh duringtime=%d",
        gs_tremos_thresh_during_time, gs_unusual_moving_thresh_during_time);

    log_i("Gsensor hardware setting: INT level=%d, tremos level=%d, measure range=%d, sample freq:%d, reading count=%d, interval=%d",
        gsensor_driver_para.interrupt_sensitivity, gsensor_driver_para.tremos_sensitivity, gsensor_driver_para.measure_range, 
        gsensor_driver_para.sample_frequecy, gsensor_driver_para.reading_count, gsensor_driver_para.interval);  

}

/*初始化车型调整角度补偿*/
static void gsensor_init_adjust_angle(void)
{

    st_angle.pitch_offset = 0;  //俯仰角偏置
    st_angle.roll_offset = 0;   //翻滚角偏置

}


void gsensor_app_driver_param_sync(void)
{
    qlsdk_sensor_support.accel_range = gsensor_driver_para.measure_range;
    qlsdk_sensor_support.cur_odr = gsensor_driver_para.sample_frequecy;
}


/*根据车型初始化参数*/
void gsensor_module_init_prara(void)
{
    gsensor_init_adjust_angle();
    gsensor_thresh_init();
    
    kf_pitch_init(&KF_pitch);
    kf_roll_init(&KF_roll);

    /* driver和应用分别存储不同参数，有重叠部分，开机时做同步 */
    gsensor_app_driver_param_sync();
}

/*设置gsensor告警阈值*/
int tbox_gsensor_set_threshold(int threshold)
{
    int ret;

    /* 入参检查 */
    if ((threshold < SENSITIVITY_LOW_LEVEL) || (threshold > SENSITIVITY_HIGH_LEVEL) && (threshold != SENSITIVITY_VERY_HIGH_LEVEL)) {
        log_e("tbox gsensor module set threshold input invalid param");
        return FAILURE;
    }

    ret = fw_gsensor_set_threshold(threshold);
    if (ret != SUCCESS)
    {
        log_e("tbox gsensor module set threshold fail");
        return ret;
    }

    log_i("tbox gsensor module set threshold ok, threshold[%d]", threshold);
    return SUCCESS;
}


int swei_gsensor_module_init(void)
{
    int ret = 0;
    
    /* 检查gsensor外设是否初始化成功 */
    ret = fw_gsensor_check();
    if (ret == FAILURE)
    {
        log_e("fw_gsensor_check err");
        return FAILURE;
    }

    /* 设置gsensor告警阀值,初始化时可以先设置一个默认值在使用过程可以动态修改*/
    //ret = tbox_gsensor_set_threshold(gsensor_driver_para.tremos_sensitivity);
    ret = tbox_gsensor_set_threshold(EVEN_MOTION_LEVEL_DEFAULT);
    if (ret == FAILURE)
    {
        log_e("tbox_gsensor_set_threshold err");
        return FAILURE;
    }

    /*range设置在8G*/
    ret = fw_gsensor_set_measure_range(gsensor_driver_para.measure_range);
    if (ret == FAILURE)
    {
        log_e("fw_gsensor_set_measure_range err");
        return FAILURE;
    }

    /*分辨率设置*/
    ret = fw_gsensor_set_sampled_frequency(gsensor_driver_para.sample_frequecy);
    if (ret == FAILURE)
    {
        log_e("fw_gsensor_set_sampled_frequency err");
        return FAILURE;
    }

    log_i("tbox gsensor module init success");

    return SUCCESS;
}


/*读初始化速度，不去除有效位，自研协议去除有效位*/
void read_intitial_speed(int tick)
{
    if (co_motor_ctrl_info_1.speed_valid)
    {
        tick_speed = (int)(co_motor_ctrl_info_1.speed);  //单位KM/H
    }
    else
    {
        tick_speed = 0;
    }

    /*速度选取40tick之前的*/
    int var = tick % SPEED_TICK_COUNT;
    speed_tick_last = speed_tick_queue[var];
    speed_tick_queue[var] = tick_speed;
 }



char get_gs_collide_machine_state(void)
{
    return gs_collide_machine;
}


/****************************************************震动告警相关***********************************************************/

/*将震动告警阈值改为极低，用于检测异动告警*/
void set_gsensor_thresh(int thresh)
{
    if(FALSE == set_gsensor_thresh_flag)
    {
        tbox_gsensor_set_threshold(thresh);
        set_gsensor_thresh_flag = true;
        log_i("set_gsensor_thresh : %d",thresh);
    }
}

/*将震动告警阈值恢复为用户设置的*/
void clear_gsensor_thresh(void)
{
    if(TRUE == set_gsensor_thresh_flag)
    {
        log_i("clear_gsensor_thresh : %d",gsensor_driver_para.tremos_sensitivity);
        tbox_gsensor_set_threshold(gsensor_driver_para.tremos_sensitivity);
        set_gsensor_thresh_flag = false;
    }
}


/*震动告警检测*/
static void collide_event_check(void)
{
    static int collide_event_last_time = 0;/*上次碰撞事件发生的时间*/
    static int collide_clear_last_time = 0;/*上次清除碰撞检查中断状态的时间*/
    int res = 0;
    int collide_event_now_time = swei_get_dev_timestamp();
    int judge = collide_event_now_time - collide_event_last_time;

    gsensor_shake_enable();

    if (judge > gs_collide_time_during) {
        res = gsensor_interrupt_check();
        if (res) {  /* 确认是否触发碰撞 */
            /*控制下次进入碰撞检查时间*/
            collide_event_last_time = collide_event_now_time; 
            /*进入一次碰撞后，下一次碰撞事件的发生需要在5s以后*/
            gs_collide_time_during = 5;
            
            // 记录collide，用于异动检测
            int var = gs_collide_event_count % 3;
            gs_collide_event_time_queue[var] = collide_event_now_time;
            gs_collide_event_count ++;
             
            if (gs_collide_machine == GSENSOR_LOW_POWER) {  /* 进入碰撞检查流程 */
                gs_collide_machine = GSENSOR_AWAIT_PANEL_PASSWORD_INPUT;
            } else {
                log_i("snsr enable detect the next shake event!\r\n");
            }
        
            gsensor_interrupt_clear();
        } else {
            /* 定期间隔5s清除震动标志，以保证震动能够稳定触发 */
            judge = collide_event_now_time - collide_clear_last_time;
            if (judge > 10) {
                clear_wom_int_flags();
                collide_clear_last_time = collide_event_now_time;
            }
       }
    } else {
        /* 未达到震动检查间隔时间 */
        res = gsensor_interrupt_check();
        if (res) {
            clear_wom_int_flags();
            gsensor_interrupt_clear();
        }
    }
}


/* 震动告警的事件处理*/
static void tremos_check(void)
{
    int ret;
    /*现在进入tremos处理的时间*/
    int tremos_now_time  = swei_get_dev_timestamp();
    /*judge判断两次震动事件间隔是否足够 */
    int judge = tremos_now_time - gs_tremos_last_time - gs_tremos_thresh_during_time;

    #warning "GSENSOR tremos_check open."
    if ( (judge > 0) || (0 == gs_tremos_last_time) ) {
        log_w("execute tremos procesing");
        
        //set_gsensor_thresh(unusual_move_thresh); //进入震动告警修改阈值为极低
        gs_tremos_last_time = tremos_now_time;

        tbox_power_set_business_flag(GSENSOR_TYPE, 1);

        /* 播放告警音频 */
        co_aud_cmd(AUD_TYPE_ALARM);
        /* 震动告警记录 */        
        dev.move_alarm_count += 1;

        /* 给服务器发送个告警数据(震动) */
        ret = swei_netProtocol_send_gsensor_warning_msg(ALARM_SHAKE);

        if ((0 != ret))
        {
            log_e("fail to send to net when gsensor tremos warning");
        } 

    }
}



/**********************************************************倾斜报警相关***************************************************************/

/*倾斜报警检测*/
static void tilt_check(float pitch, float roll, int tick, struct FW_GSENSOR_INFO_STRU * kg_info, struct FW_GYROSCOPE_INFO_STRU * _kgy_info)
{

    /*速度获取函数*/
    if (speed_tick_last > tilt_mng_thresh.vel)
    {
        float acc_xy = 1000 * sqrt(kg_info->acc_x*kg_info->acc_x+ kg_info->acc_y*kg_info->acc_y);
        float gyro_x = _kgy_info->gyro_x;  //ROLL

        float tilt_acc_judge_val = acc_xy - tilt_mng_thresh.acc;
        float tilt_gyro_judge_val = gyro_x - tilt_mng_thresh.gyro;

        //tilt acc_xy:778.6774, gyro_xy:116.7026, -28.9686 -113.0500
        //log_d("tilt acc_xy:%.4f, %.4f %.4f. gyro_xy:%.4f, %.4f %.4f", acc_xy, kg_info->acc_x, kg_info->acc_y, gyro_x, _kgy_info->gyro_x, _kgy_info->gyro_y);
        
        if( (tilt_acc_judge_val > 0) && (tilt_gyro_judge_val > 0)) 
        {
            /*满足角度条件，触发tilt状态*/
            tilt_state_now = TILT_RISK;

            /*第一次触发需要清零*/
            if (0 == tilt_count)
            {
                tilt_tick_first = tick;
            }
            
            tilt_count++;

            if(TRUE == debug_status)
            {
                log_d("change para:[angle:%.4f,%.4f];[tick:%d,%d];[kacc:%.4f,%.4f,%.4f,%.4f,%.4f];[kgyro:%.4f,%.4f,%.4f];\
                                        [acc:%.4f,%.4f,%.4f,%.4f,%.4f];[gyro:%.4f,%.4f,%.4f];\
                                        [offset:%f,%f],[speed:%d]", \
                    pitch, roll, \
                    tick, tilt_tick_first,\
                    st_rate_acc.acc_rate_xy, st_acc.acc_xyz,kg_info->acc_x,kg_info->acc_y,kg_info->acc_z,\
                    kgy_info.gyro_x,kgy_info.gyro_y,kgy_info.gyro_z,\
                    st_rate_acc.acc_rate_xy, st_acc.acc_xyz,g_info.acc_x,g_info.acc_y,g_info.acc_z,\
                    gy_info.gyro_x,gy_info.gyro_y,gy_info.gyro_z,\
                    st_angle.pitch_offset,st_angle.roll_offset,speed_tick_last);

            }
            else
            {
                //暂时不做应用
            }
            
            /*防抖后，判断持续触发达到tilt*/
            if ((tilt_count > tilt_mng_thresh.time) && ((tick - tilt_tick_first) < 100))
            {
                int tilt_warnning_now_time = swei_get_dev_timestamp();
                /*开机就触发 或者 5分钟内只播报一次*/
                if((0 == s_tilt_warnning_last_time) || (tilt_warnning_now_time - s_tilt_warnning_last_time > gs_tilt_warnning_during_time))
                {
                    log_i(" tilt warnnig ");
                    /* 播放告警音频 */
                    co_aud_cmd(AUD_TYPE_ALARM);

                    /* 翻滚告警记录 */        
                    dev.tilt_alarm_count += 1;

                    /* 上传倾斜报警 */
                    swei_netProtocol_send_gsensor_warning_msg(ALARM_TILT);

                    s_tilt_warnning_last_time = tilt_warnning_now_time;
                }
                tilt_count = 0;
            }
            /*长时间未触发，直接将count清零*/
            if ((tick - tilt_tick_first) > 100)
            {
                tilt_count = 0;
            }
            tilt_state_last = TILT_RISK;
            return;
        }
        else
        {
            /*已经满足速度，未满足角速度，加速度、时间未定*/
            tilt_state_now = TILT_SAFE;

            /*如果两次未触发tilt，将count清零*/
            if ((tilt_state_now == TILT_SAFE) && (tilt_state_last == TILT_SAFE))
            {
                tilt_count = 0;
            }

            /*长时间未触发，直接将count清零*/
            if ((tick - tilt_tick_first) > 100)
            {
                tilt_count = 0;
            }
            tilt_state_last = TILT_SAFE;
        }
    }    
    else
    {
        /*未满足速度状态均为safe*/
        tilt_state_now = TILT_SAFE;

        /*如果两次未触发tilt，将count清零*/
        if ((tilt_state_now == TILT_SAFE) && (tilt_state_last == TILT_SAFE))
        {
            tilt_count = 0;
        }

        /*长时间未触发，直接将count清零*/
        if ((tick - tilt_tick_first) > 100)
        {
            tilt_count = 0;
        }
        tilt_state_last = TILT_SAFE;
    }


}

/**********************************************翻转报警相关*******************************************************/

/*翻转报警检测*/
static void overturn_check(float pitch, float roll, int tick, struct FW_GSENSOR_INFO_STRU * kg_info, struct FW_GYROSCOPE_INFO_STRU * _kgy_info)
{
    int overturn_warnning_now_time = swei_get_dev_timestamp();

    float acc_xy = 1000 * sqrt(kg_info->acc_x*kg_info->acc_x+ kg_info->acc_y*kg_info->acc_y);
    
    /*速度获取函数*/
    if (speed_tick_last > overturn_mng_thresh.vel)
    {
        /* 加速度 */
        /* 角速度 */ 
        float overturn_acc_judge_val = acc_xy - overturn_mng_thresh.acc;
        //float overturn_gyro_x_judge_val = _kgy_info->gyro_x - overturn_mng_thresh.gyro;
        float overturn_gyro_y_judge_val = _kgy_info->gyro_y - overturn_mng_thresh.gyro;
        //float overturn_gyro_z_judge_val = _kgy_info->gyro_z - overturn_mng_thresh.gyro;
        
        if( (overturn_acc_judge_val > 0) 
            && (/* (overturn_gyro_x_judge_val > 0) || */ (overturn_gyro_y_judge_val > 0) /* || (overturn_gyro_z_judge_val > 0) */) ) 
        {
            overturn_state_now = OVERTURN_RISK;

            if (overturn_count == 0)
            {
                overturn_tick_first = tick;
            }

            overturn_count ++;
            if(TRUE == debug_status)
            {
                log_d("overturn para:[angle:%.4f,%.4f];[tick:%d,%d];[kacc:%.4f,%.4f,%.4f,%.4f];[kgyro:%.4f,%.4f,%.4f];\
                    [acc:%.4f,%.4f,%.4f,%.4f];[gyro:%.4f,%.4f,%.4f];\
                    [offset:%f,%f];[speed:%d]",\
                    pitch, roll,\
                    tick,overturn_tick_first,\
                    st_acc.acc_xyz,kg_info->acc_x, kg_info->acc_y, kg_info->acc_z, \
                    kgy_info.gyro_x,kgy_info.gyro_y,kgy_info.gyro_z,\
                    st_acc.acc_xyz,g_info.acc_x, g_info.acc_y, g_info.acc_z, \
                    gy_info.gyro_x,gy_info.gyro_y,gy_info.gyro_z,\
                    st_angle.pitch_offset,st_angle.roll_offset,\
                    speed_tick_last);
            }
            else
            {
                //暂时不做应用
            }

            if ((overturn_count > overturn_mng_thresh.time) && ((tick - overturn_tick_first) < 40))
            {  
                log_i("overturn acc is ok");
                
                /*启动后触发则告警，或者三分钟内只进行一次云平台上传*/
                if((s_overturn_warning_last_time == 0) || (overturn_warnning_now_time - s_overturn_warning_last_time > gs_overturn_warnning_during_time))
                {
                    log_i("overturn check success ");
                    /* 播放告警音频 */
                    co_aud_cmd(AUD_TYPE_ALARM);
                    
                    /* 翻滚告警记录 */        
                    dev.roll_move_alarm_count += 1;

                    /* 上传翻转报警 */
                    swei_netProtocol_send_gsensor_warning_msg(ALARM_TUMBLING);
                    
                    s_overturn_warning_last_time = overturn_warnning_now_time;
                }
                overturn_count = 0;
                
            }

            /*达到速度、角度要求后，长时间也需要将count 清零*/
            if ((tick - overturn_tick_first) > 100)
            {
                overturn_count = 0;
            }
            overturn_state_last = OVERTURN_RISK;
            return;
        }
        else
        {
            /*达到速度要求，没有达到角度的要求，加速度和时间待定*/
            overturn_state_now = OVERTURN_SAFE;

            if ((overturn_state_now == OVERTURN_SAFE) && (overturn_state_last == OVERTURN_SAFE))
            {
                overturn_count = 0;
            }

            if ((tick - overturn_tick_first) > 100)
            {
                overturn_count = 0;
            }
        }
    }
    else
    {
        /*没达到速度的要求，加速度、角度条件待定*/
        overturn_state_now = OVERTURN_SAFE;

        if ((overturn_state_now == OVERTURN_SAFE) && (overturn_state_last == OVERTURN_SAFE))
        {
            overturn_count = 0;
        }

        if ((tick - overturn_tick_first) > 100)
        {
            overturn_count = 0;
        }
    }
    return;
}

/********************************************异动告警相关***************************************************/
#include "misc/misc_system.h"
/*gps线程*/
void swei_gsensor_check_gps_move_handler(void *arg)
{
    int count = 0;
    int latitude_diff = 0;
    int longitude_diff = 0;
    double val = 0;
    bool alarm_valid = false;
    int start_time = swei_get_dev_timestamp();//起始时间
    int now_time = swei_get_dev_timestamp();//现在时间时间
    memset(&gps_check_info, 0, sizeof(gps_check_info));

    //swei_gps_business_open(BIT_GSENSOR);//开启gps

    log_i("tbox thread run [thread_id:%lu]", xTaskGetCurrentTaskHandle());
    log_i("start check gps move...");
    //pthread_detach(pthread_self()); // 跑完后释放线程

    unsigned int old_timestamp = 0;
    unsigned int new_timestamp = 0;
    
    int latitude = 0;
    int longitude = 0;
    
    int latitude_old = 0;//用于简单滤波
    int longitude_old = 0;

    gs_gps_check_move_thread_flag = true;//开始检测
    while (1)
    {
        if(dev.auth_state)
        {
            log_i("gps checking close,user open!");
            break;
        }
        //手动关闭循环
        if (test_over_flag == 1)
        {
            test_over_flag = 0;
            break;
        }
        //超时计时(最多预留10S定位时间)
        now_time = swei_get_dev_timestamp();//现在时间时间
        if (now_time - start_time >= UNUSUAL_MOVE_CHECK_TIME)
        {
            log_e("max[%d][%d]", gps_check_info.latitude_max, gps_check_info.longitude_max);
            log_e("min[%d][%d]", gps_check_info.latitude_min, gps_check_info.longitude_min);
            log_e("diff [%d] [%d] = val [%lf]", latitude_diff, longitude_diff, val);
            log_e("check timer out! break loop.");
            break;
        }
        vTaskDelay(_MS2TICKS(2*1000)); // 2s检测一次

        // 进行检测
        struct FW_GPS_INFO_STRU info;
        memset(&info, 0, sizeof(info));
        if (gnss_power_status_get() == false || 0 != swei_gps_get_info(&info))
        {
            log_e("swei_gps_get_info fail![%d]", gnss_power_status_get());
            continue;
        }
        
        if ((info.latitude == 0 && info.longitude == 0) ||
            info.satellites_num <= 2 /*|| info.satellites_num > 24*/)
        {
            log_w("check gps info err. lat %.06f  long %.06f [num %u]", info.latitude, info.longitude, info.satellites_num);
            continue;
        }
        // info为度分格式 lat:2239.625125, lng:11407.531703
        latitude = (int)(info.latitude * MAGNIFICATION / 100);
        longitude = (int)(info.longitude * MAGNIFICATION / 100);
        log_i("2 lat %.06f  long %.06f [num %u]", latitude, longitude, info.satellites_num);//不是零
        
        if (false == gps_check_info.is_located)
        {
        
            if (count < 5) // 过滤前4个点(8S)
            {
                count++;
            }
            else 
            {
                new_timestamp = (unsigned int)get_utctime_timestamp(info.utc_time);
                if (new_timestamp == old_timestamp)
                {
                    old_timestamp = new_timestamp;
                    continue;
                }

                old_timestamp = new_timestamp;
                gps_check_info.is_located = true;       // 已定位
                // 后5位为分，后续需要转化为小数点，即除以（60*100000）
                gps_check_info.latitude_max = latitude;
                gps_check_info.latitude_min = latitude;
                gps_check_info.longitude_max = longitude;
                gps_check_info.longitude_min = longitude;
                latitude_old = latitude;
                longitude_old = longitude;
            }
        }
        else if( (count++ % 5) == 0)  //之后10S, 20S, 30S 每10S取一个点
        {
            latitude = latitude / 2 + latitude_old / 2; //简单滤波
            longitude = longitude /2 + longitude_old / 2;
            latitude_old = latitude;
            longitude_old = longitude;
            //int d = 2 * r * arcsin(sqrt(sin²((lat₂ - lat₁)/2) + cos(lat₁) * cos(lat₂) * sin²((lon₂ - lon₁)/2)))
            gps_check_info.latitude_max = (latitude > gps_check_info.latitude_max) ? latitude : gps_check_info.latitude_max;
            gps_check_info.latitude_min = (latitude < gps_check_info.latitude_min) ? latitude : gps_check_info.latitude_min;
            gps_check_info.longitude_max = (longitude > gps_check_info.longitude_max) ? longitude : gps_check_info.longitude_max;
            gps_check_info.longitude_min = (longitude < gps_check_info.longitude_min) ? longitude : gps_check_info.longitude_min;

            // 后5位为分，需要转化为小数点，即除以（60*100000）
            // diff计算的时候放大100000(MAGNIFICATION)
            latitude_diff = (gps_check_info.latitude_max / MAGNIFICATION - gps_check_info.latitude_min / MAGNIFICATION) * MAGNIFICATION / 100+
                                (gps_check_info.latitude_max % MAGNIFICATION - gps_check_info.latitude_min % MAGNIFICATION) / 60;
            longitude_diff = (gps_check_info.longitude_max / MAGNIFICATION - gps_check_info.longitude_min / MAGNIFICATION) * MAGNIFICATION / 100+
                                 (gps_check_info.longitude_max % MAGNIFICATION - gps_check_info.longitude_min % MAGNIFICATION) / 60;

            val = sqrt(latitude_diff * latitude_diff + longitude_diff * longitude_diff);

            log_i("diff [%d] [%d] = val [%lf]", latitude_diff, longitude_diff, val);
            
            //阈值为经纬度单位，约20M
            if (val > MOVE_THRESHOLD)
            {
                log_e("val over threshold[%d], warning", MOVE_THRESHOLD);

                alarm_valid = true;
                
                /* 通知仪表报警 */
                co_aud_cmd(AUD_TYPE_ALARM);

                /* 异动告警记录 */        
                dev.unusal_move_alarm_count += 1;

                /* 给服务器发送个告警数据(异动) */
                int ret = swei_netProtocol_send_gsensor_warning_msg(ALARM_ABNORMAL_MOVE);
                if ((0 != ret))
                {
                    log_e("fail to send to net when gsensor was unusual warning");
                }  else {
                    /* 一次异动告警带一次GPS LBS信息 */
                    swei_location_set_force_report(REPORT_GPS);
                    swei_location_push_gps();
                    
                    swei_location_set_force_report(REPORT_LBS);
                    swei_location_push_lbs();
                }

                break;
            }
        }
    }
   
    log_i("over check gps move...");
    if(alarm_valid == false) //gps距离条件不满足
    {
        /*未告警成功，允许下次异动马上判断*/
        gs_unusual_move_warn_last_time = 0;
    }
    
    unusual_move_set_bussiness_flag = FALSE;
    gs_gps_check_move_thread_flag = false;

    /*结束检测*/
    //swei_gps_business_close(BIT_GSENSOR);
    
    /* 删除自身 */
    vTaskDelete(NULL);
    
    return;
}

/*开启检测gps线程*/

static TaskHandle_t snsr_mov_check_handle;

int swei_gsensor_start_check_gps_move(void)
{
    // 移动检查中，不重复创建
    if (gs_gps_check_move_thread_flag == true)
    {
        log_i("swei_gsensor_check_gps_move_handler is working...");
        return 0;
    }

    // 创建移动检查现场
    if(pdPASS != xTaskCreate(swei_gsensor_check_gps_move_handler, "SNSR_MOV_CHK", 1024 * 6, NULL, osPriorityNormal, &snsr_mov_check_handle))
    {
        //Error_Handler();
        log_e("SNSR_MOV_CHK thread_create fail.");
        return -1;
    }

    unusual_move_set_bussiness_flag = TRUE; //此标志位作用为防止异动检测过程中遇到RTC等导致睡眠情况的发生
    tbox_power_set_business_flag(GSENSOR_TYPE, 1);
    return 0;
}


/*异动告警检测*/
static void unusual_move_check()
{
    int judge = gs_collide_event_count % 3;
    int gs_unusual_move_log_time = swei_get_dev_timestamp();

    /* GPS测量中 */
    if(gs_gps_check_move_thread_flag == TRUE)
    {
        /* 异动触发检查中 */
        return;
    }

    /*判断是否有三次collide_event_count*/
    switch (judge)
    {
        case 0:
        /* 30S内3次连续震动，触发异动告警测量 */
        if (gs_collide_event_time_queue[2] - gs_collide_event_time_queue[0] < MOVE_TIME_DURING)
        {
            int unusual_move_warn_now_time = swei_get_dev_timestamp();
            /*刷屏log，4s一次*/
            if(gs_unusual_move_log_time - gs_unusual_move_log_time_prev > 4)
            {
                gs_unusual_move_log_time_prev = gs_unusual_move_log_time;
                log_w("unusual move ------- round 3 ");
            }
            
            /*第三次碰撞事件发生，先把次数清零，再判断第一次和第二次的时间*/
            gs_collide_event_count = 0;

            /* 相邻两次告警触发大于5min，启动GPS距离测量 */
            if( (unusual_move_warn_now_time - gs_unusual_move_warn_last_time > gs_unusual_moving_thresh_during_time) \
              || (0 == gs_unusual_move_warn_last_time) )
            {
                log_w("execute unusual moving procesing");

                //clear_gsensor_thresh(); //恢复用户震动告警阈值
                
                /*开启gps检测线程*/
                swei_gsensor_start_check_gps_move();
                gs_unusual_move_warn_last_time = unusual_move_warn_now_time;
                /*异动事件后，下次发生碰撞的检测需要根据震动告警的时间来确定*/
                gs_collide_time_during = gs_tremos_thresh_during_time - (unusual_move_warn_now_time - gs_tremos_last_time);
            }
            else
            {
                if(gs_unusual_move_log_time - gs_unusual_move_log_time_prev > 4)
                {
                    gs_unusual_move_log_time_prev = gs_unusual_move_log_time;
                    /*五分钟以内的算作无效异动报警*/
                    log_w(" Meeting actional conditions for unusual moving warnings, but it's less than 5 minutes since the last");
                }
            }
        }
        else
        {
            gs_collide_event_count = 1;
            gs_collide_event_time_queue[0] = gs_collide_event_time_queue[2];
            log_w("The time among these three rounds of unusual movement is more than 30 seconds,so it's unusual move ------- round 1");
        }
            break;
        
        case 2:
            if(gs_unusual_move_log_time - gs_unusual_move_log_time_prev > 4)
            {
                gs_unusual_move_log_time_prev = gs_unusual_move_log_time;
                log_w("unusual move ------- round 2");          
            }  
            
            if(gs_collide_event_time_queue[1] - gs_collide_event_time_queue[0] > MOVE_TIME_DURING)
            {
                gs_collide_event_count = 1;
                gs_collide_event_time_queue[0] = gs_collide_event_time_queue[1];
                log_w("The time between these two round of unusual movement is more than 30 seconds,so it's unusual move ------- round 1");
            }
            break;
            
        case 1:
            if(gs_unusual_move_log_time - gs_unusual_move_log_time_prev > 4)
            {
                gs_unusual_move_log_time_prev = gs_unusual_move_log_time;
                log_w("unusual move ------- round 1");          
            } 
            break;
        default:
            break;
            
    } 

    if(TRUE == gs_gps_check_move_thread_flag)
    {   /* 异动触发检查中 */
        log_i("gps checking");
        return ;
    }
}


/*获取声音播放状态*/
bool swei_get_is_collide_sound_playing(void)
{
    return is_collide_sound_playing;
}

/**********************************************计算************************************************/
/*计算俯仰角angle_pitch,翻转角angle_roll*/
static void calc_angle_and_acc()
{
    /*因为TBox的安装方向，角速度xy坐标可能需要对换*/
    if(get_gsensor_change_gyro_xy()) {
        float gyro_temp = gy_info.gyro_x;
        gy_info.gyro_x = gy_info.gyro_y;
        gy_info.gyro_y = gyro_temp;
    }

    /*弧度转为角度*/
    gy_info.gyro_y = gy_info.gyro_y * 57.4;
    gy_info.gyro_x = gy_info.gyro_x * 57.4 ;
    gy_info.gyro_z = gy_info.gyro_z * 57.4 ;
    
    /*高通滤波求角速度*/
    kgy_info.gyro_x = highPassFilter(0.01, gy_info.gyro_x);
    kgy_info.gyro_y = highPassFilter(0.015, gy_info.gyro_y);
    kgy_info.gyro_z = highPassFilter(0.025, gy_info.gyro_z);

    /*姿态解算*/
    float acc_xy = sqrt(kg_info.acc_y*kg_info.acc_y+ kg_info.acc_z*kg_info.acc_z);
    kg_info.acc_x = 0- kg_info.acc_x;
    float angle_pitch_m = atan2(kg_info.acc_x, acc_xy) * STRAIGHT_ANGLE / PI;
    kg_info.acc_x = 0- kg_info.acc_x;
    float angle_roll_m = atan2(kg_info.acc_y, kg_info.acc_z) * STRAIGHT_ANGLE / PI;

    KF_roll = kalman_filter(angle_roll_m, kgy_info.gyro_x, KF_roll, st_acc);
    KF_pitch = kalman_filter(angle_pitch_m, kgy_info.gyro_y, KF_pitch, st_acc);
    /*pitch角度*/
    KF_pitch.angle = 0 - KF_pitch.angle;
    st_angle.angle_roll = KF_roll.angle + st_angle.roll_offset;
    st_angle.angle_pitch = KF_pitch.angle + st_angle.pitch_offset;
    KF_pitch.angle = 0 - KF_pitch.angle;

    if (st_angle.angle_pitch > 180)
    {
        st_angle.angle_pitch -= 360;
    }
    else if (st_angle.angle_pitch < -180)
    {
        st_angle.angle_pitch += 360;
    }

    if (st_angle.angle_roll > 180)
    {
        st_angle.angle_roll -= 360;
    }
    else if (st_angle.angle_roll < -180)
    {
        st_angle.angle_roll += 360;
    }
}

/**
 * @brief SNSR状态
 */
const char *SNSR_STEP[GSENSOR_AWAIT_PANEL_PASSWORD_INPUT_COLLIDE + 1] = {
    "GSENSOR_NORMAL_WORK",
    "GSENSOR_LOW_POWER",
    "GSENSOR_AWAIT_PANEL_PASSWORD_INPUT",
    "GSENSOR_AWAIT_PANEL_PASSWORD_INPUT_COLLIDE"
};


void tremos_param_init(void)
{
    gs_tremos_last_time = 0;
}

void unusual_move_param_init(void)
{
    memset(gs_collide_event_time_queue, 0, sizeof(gs_collide_event_time_queue));
    gs_collide_event_count = 0;
    gs_unusual_move_warn_last_time = 0;
    gs_unusual_move_log_time_prev = 0;
}

void tilt_param_init(void)
{
    tilt_count = 0;                 /*倾斜检测的总次数*/
    tilt_tick_first = 0;            /*连续检测时，第一次倾斜的时间*/
    tilt_state_now = TILT_SAFE;     /*当前检测时的状态*/
    tilt_state_last = TILT_SAFE;    /*上次检测时的状态*/
    s_tilt_warnning_last_time = 0;  /*上次倾斜报警的时间，目前要求三分钟只报一次*/
}

void overturn_param_init(void)
{
    overturn_count = 0;
    overturn_tick_first = 0;
    overturn_state_now = TILT_SAFE;
    overturn_state_last = TILT_SAFE;
    s_overturn_warning_last_time = 0;
}


/**
 * @brief  根据密码授权状态和CO MCU开关机状态同步设备状态和Gsensor状态     
           密码授权状态:受开关机影响，影响设备正常工作状态或者未鉴权状态；
           非设备正常工作状态下,Gsensor进入低功耗模式(根据设备上下电和密码授权状态判断震动或异动)，设备正常工作状态下Gsensor进入正常工作模式(判断侧倾和翻滚)
 * @param  none
 * @retval None
 */
void gs_state_sync(void)
{
    static bool gs_state_powerup = false;
    static bool prev_power_state = false;

    /* CO MCU开机,更新鉴权装填 */
    if (!is_ebike_mcu_off()) {
        dev.power_state = true;
        if (get_auth_status()) {//)
            dev.auth_state = true;   
        } else {
            dev.auth_state = false;   
        }
    } else {
        dev.power_state = false;
    }
    
    if( prev_power_state != dev.power_state) {

        /* 仪表开机，重置倾斜和翻转检测 */
        if (true == dev.power_state) {
            
            tilt_param_init();
            overturn_param_init();
            log_i("CO_MCU power on, reset tilt and overturn check param.");

        /* 仪表关机，重置震动和异动检测 */    
        } else {
           
            tremos_param_init();
            unusual_move_param_init();
            log_i("CO_MCU power off, reset tremos and unusual move check param.");
        }

        prev_power_state = dev.power_state;
    }


    /* 首次上电状态打印 */
    if (false == gs_state_powerup) 
    {
        log_i("Startup CO MCU state:%s, Gsensor state:%s, IOT state:%s.", \
            dev.power_state ? "POWER ON" : "POWER OFF", \
            SNSR_STEP[gs_collide_machine] , \
            dev.auth_state ? "AUTH PASS" : "WAIT AUTH");

        gsensor_shake_enable(); // 默认开机进入震动检测低功耗阶段

        gs_state_powerup = true;    
    }
}

/***************************************************************gsensor线程***************************************************************/
void swei_proc_gsensor_info_handler(void)
{
    /**
     * @brief Construct a new gs state sync object
     */
        gs_state_sync();
    
        /*以下操作无论什么状态都应该进行,读取传感器数据 */
        read_initial_acc_gyro(&g_info, &gy_info, gsensor_driver_para);        
        {
            static unsigned int disp_intv = 0;

            if (timeout_ms(disp_intv, 200)  && (TRUE == debug_status)) {
                log_i("acc_x:%.3fg, acc_y:%.3fg, acc_z:%.3fg, temp:%.1f, gyro_x:%.3frad/s, gyro_y:%.3frad/s, gyro_z:%.3frad/s.\r\n", \
                    g_info.acc_x, g_info.acc_y, g_info.acc_z, g_info.temp, gy_info.gyro_x, gy_info.gyro_y, gy_info.gyro_z);
                disp_intv = getms();
            }
        }

        read_intitial_speed(tick); 
        tick++;

    static int last_step = GSENSOR_LOW_POWER;
    static uint8_t last_power_state = 0;
    static uint8_t last_auth_state = 0;
    if ( (gs_collide_machine != last_step) || (dev.power_state != last_power_state) || (dev.auth_state != last_auth_state) ) {
        log_i("CO MCU state:%s, Gsensor state:%s, IOT state:%s.", \
            dev.power_state ? "POWER ON" : "POWER OFF", \
            SNSR_STEP[gs_collide_machine] , \
            dev.auth_state ? "AUTH PASS" : "WAIT AUTH");

        last_step = gs_collide_machine;
        last_power_state = dev.power_state;
        last_auth_state = dev.auth_state;
    } 

    switch(gs_collide_machine)
    {
        case GSENSOR_AWAIT_PANEL_PASSWORD_INPUT: {   /* 待鉴权阶段 */
            
            if (dev.power_state)    /* 设备开机，进入等待鉴权阶段 */ {
                if (dev.auth_state) {

                    /* !!!sensor检测倾斜翻滚，需要关闭低功耗模式 */
                    gsensor_shake_disable();
                    
                    gs_collide_machine = GSENSOR_NORMAL_WORK;
                    log_i("Gsensor enter :normal work.");
                } else {
                    gs_collide_machine = GSENSOR_AWAIT_PANEL_PASSWORD_INPUT_COLLIDE;
                    log_i("Gsensor enter :wait for athentification.");
                }
            } else {
                dev.alarm_state = ALARM_MOVE_MASK | ALARM_UNUSUAL_MOVE_MASK;

                /* !!!sensor中断唤醒，要判断震动强度是否符合设置阈值，否则不算震动和异动 */
                if(MOTION_STATUS_ON == motion_detech_variation()) {
                    // 震动事件处理
                    tremos_check();
                    // 判断是否触发异动;
                    unusual_move_check();
                }

                /* 处理完重新进入低功耗 */
                gs_collide_machine = GSENSOR_LOW_POWER;
            }
        }
        break;

        case GSENSOR_AWAIT_PANEL_PASSWORD_INPUT_COLLIDE: {
            if (dev.auth_state) {
                /* !!!sensor检测倾斜翻滚，需要关闭低功耗模式 */
                gsensor_shake_disable();
                gs_collide_machine = GSENSOR_NORMAL_WORK;
                log_i("Gsensor enter :normal work.");
            } else {
                dev.alarm_state = ALARM_UNUSUAL_MOVE_MASK;

                /* 判断震动强度是否符合设置阈值，否则不算异动 */
                if(MOTION_STATUS_ON == motion_detech_variation()) {
                    unusual_move_check();
                }
            }
        }
        break;

        /* 骑行状态，倾斜和翻滚检测打开 */
        case GSENSOR_NORMAL_WORK:
        {
            if (dev.power_state == false) {
                /* 上电状态下，关闭主电切换到低功耗 */
                gs_collide_machine = GSENSOR_LOW_POWER;

                /* !!!sensor检测震动和异动，需要打开低功耗模式 */
                gsensor_shake_enable();
            } else {

                /*计算*/
                cal_acc_rate(&g_info,&kg_info, &st_rate_acc, &st_acc);
                calc_angle_and_acc();
                
                /*判断倾斜翻滚*/
                dev.alarm_state = ALARM_TILT_MASK | ALARM_ROLL_MASK;
                
                overturn_check(st_angle.angle_pitch, st_angle.angle_roll, tick, &kg_info, &kgy_info);
                tilt_check(st_angle.angle_pitch, st_angle.angle_roll, tick, &kg_info, &kgy_info);
            }
        }
        break;
            
        /* 下主电(休眠)唤醒时，震动和异动检测打开 */
        case GSENSOR_LOW_POWER:
        {
            dev.alarm_state = ALARM_MOVE_MASK | ALARM_UNUSUAL_MOVE_MASK;
            /*进行碰撞事件的检测*/
            collide_event_check();
        }     
        break;

        default:
            log_e("status: %d error", gs_collide_machine);
            break;
    }

    /* 50ms运行一次，每次read_initial_data读50个数据取平均，从输出的结果来看128ms左右刷一次数据出来*/
    usleep(gsensor_driver_para.interval * 1000);

    return;
}

int gsensor_interrupt_flag = 0;
/**
 * @brief 中断回调函数
 */
void gsensor_interrupt_handler(void)
                {
    gsensor_interrupt_flag = 1;
}
/**
 * @brief 检查中断标志
 * @return int 
 */
int gsensor_interrupt_check(void)
{
    return gsensor_interrupt_flag;
}

/**
 * @brief 清除中断标志
 */
void gsensor_interrupt_clear(void)
{
    gsensor_interrupt_flag = 0;
}

static uint8_t is_snsr_lowpower = 0;
/**
 * @brief 震动使能
 */
void gsensor_shake_enable(void)
{
    taskENTER_CRITICAL();
    if (!is_snsr_lowpower) {
        is_snsr_lowpower = 1;
        sensor_lowpower_initialize();
    }
                    
    clear_wom_int_flags();
    taskEXIT_CRITICAL();
}

/**
 * @brief 震动失能
 */
void gsensor_shake_disable(void)
{
    taskENTER_CRITICAL();
    sensor_initialize();
    is_snsr_lowpower = 0;
    taskEXIT_CRITICAL();
}

/**
 * @brief 检测使能
 */
uint32_t gsensor_alarm_state_get(void)
{        
    return dev.alarm_state;
} 
        
/**
 * @brief 异动状态检测中，关联休眠逻辑，如果为true，则不进入休眠
 * @return true  - 进行中
 * @return false 
 */
bool gsensor_unusal_move_check(void)
{
    return unusual_move_set_bussiness_flag;
}


/* 默认安装状态0时，水平横向右为acc_Y(gyro_X)轴，前方向为aac_X(gyro_Y)轴 */
/* 需要调换一下gyro变成 水平横向右为acc_Y(gyro_Y)轴，前方向为aac_X(gyro_X)轴*/
static char gsensor_change_gyro_xy = 1;
bool get_gsensor_change_gyro_xy(void)
{
    return (gsensor_change_gyro_xy != 0);
}


#include "lettershell/shell.h"
SHELL_EXPORT_VAR(SHELL_CMD_PERMISSION(0)|SHELL_CMD_TYPE(SHELL_TYPE_VAR_INT), gsensor_state, &gs_collide_machine, var for gsensor state machine);

SHELL_EXPORT_VAR(SHELL_CMD_PERMISSION(0)|SHELL_CMD_TYPE(SHELL_TYPE_VAR_CHAR), gsensor_data, &debug_status, var for gsensor data print);

int32_t set_gsensor_sensitive_level(uint32_t level)
{
    tbox_gsensor_set_threshold(level);

    return 0;
}
SHELL_EXPORT_CMD(SHELL_CMD_PERMISSION(0) | SHELL_CMD_TYPE(SHELL_TYPE_CMD_FUNC) | SHELL_CMD_DISABLE_RETURN,
                 set_gsensor_sensitive, set_gsensor_sensitive_level, set gsensor sensitive level);

int32_t set_gsensor_pitch_offset(uint32_t offset)
{
    st_angle.pitch_offset = (float)offset;

    return 0;
}
SHELL_EXPORT_CMD(SHELL_CMD_PERMISSION(0) | SHELL_CMD_TYPE(SHELL_TYPE_CMD_FUNC) | SHELL_CMD_DISABLE_RETURN,
                 set_gsensor_pitch_offset, set_gsensor_pitch_offset, set gsensor pitch offset);

 int32_t set_gsensor_roll_offset(uint32_t offset)
 {
     st_angle.roll_offset = (float)offset;
 
     return 0;
 }
 SHELL_EXPORT_CMD(SHELL_CMD_PERMISSION(0) | SHELL_CMD_TYPE(SHELL_TYPE_CMD_FUNC) | SHELL_CMD_DISABLE_RETURN,
                  set_gsensor_roll_offset, set_gsensor_roll_offset, set gsensor roll offset);

SHELL_EXPORT_VAR(SHELL_CMD_PERMISSION(0)|SHELL_CMD_TYPE(SHELL_TYPE_VAR_CHAR), gsensor_change_gyro_xy, &gsensor_change_gyro_xy, var for gsensor change gyro xy axis);

int32_t set_gsensor_tilt_thresh_acc(uint32_t val)
{
    tilt_mng_thresh.acc = (float)val;

    return 0;
}

SHELL_EXPORT_CMD(SHELL_CMD_PERMISSION(0) | SHELL_CMD_TYPE(SHELL_TYPE_CMD_FUNC) | SHELL_CMD_DISABLE_RETURN,
                 set_gsensor_tilt_thresh_acc, set_gsensor_tilt_thresh_acc, set gsensor tilt thresh acc);


 int32_t set_gsensor_tilt_thresh_gyro(uint32_t val)
 {
     tilt_mng_thresh.gyro = val;
 
     return 0;
 }
 SHELL_EXPORT_CMD(SHELL_CMD_PERMISSION(0) | SHELL_CMD_TYPE(SHELL_TYPE_CMD_FUNC) | SHELL_CMD_DISABLE_RETURN,
                  set_gsensor_tilt_thresh_gyro, set_gsensor_tilt_thresh_gyro, set gsensor tilt thresh gyro);

int32_t set_gsensor_overturn_thresh_acc(uint32_t val)
{
    overturn_mng_thresh.acc = (float)val;

    return 0;
}

SHELL_EXPORT_CMD(SHELL_CMD_PERMISSION(0) | SHELL_CMD_TYPE(SHELL_TYPE_CMD_FUNC) | SHELL_CMD_DISABLE_RETURN,
               set_gsensor_overturn_thresh_acc, set_gsensor_overturn_thresh_acc, set gsensor overturn thresh acc);


int32_t set_gsensor_overturn_thresh_gyro(uint32_t val)
{
     overturn_mng_thresh.gyro = val;

     return 0;
}
SHELL_EXPORT_CMD(SHELL_CMD_PERMISSION(0) | SHELL_CMD_TYPE(SHELL_TYPE_CMD_FUNC) | SHELL_CMD_DISABLE_RETURN,
                set_gsensor_overturn_thresh_gyro, set_gsensor_overturn_thresh_gyro, set gsensor overturn thresh gyro);


int32_t set_platform_default_param(void)
{
     load_platform_config(1);

     return 0;
}
SHELL_EXPORT_CMD(SHELL_CMD_PERMISSION(0) | SHELL_CMD_TYPE(SHELL_TYPE_CMD_FUNC) | SHELL_CMD_DISABLE_RETURN,
                set_platform_default_param, set_platform_default_param, set gsensor and platform default param and save);


